%% PMSM Parameters
PMSM_Param.Rs = 0.01; % Resistance of the stator windings
PMSM_Param.Ld = 2e-4; % d-axis inductances
PMSM_Param.Lq = 4e-4; % q-axis inductances
PMSM_Param.lambda = 0.0064; % Amplitude of the flux induced by the permanent magnets of the rotor in the stator phases
PMSM_Param.p = 4; % Numbers of Pole Pairs
PMSM_Param.F = 9e-5; % Combined viscous friction of rotor and load
PMSM_Param.Irated = 40;
Udc = 24;

%% Identify Operating Points (id, iq)
syms id iq
eqns = [PMSM_Param.lambda*id + (id^2-iq^2)*(PMSM_Param.Ld-PMSM_Param.Lq) == 0, ...
    id^2 + iq^2 == imax^2];
S = solve(eqns, [id, iq], 'Real', true);
index = (double(S.id)<0) & (double(S.iq)>0);
id = double(S.id(index)); iq = double(S.iq(index));
omega_e1 = sqrt(Udc^2 / 3 / ((PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2));
rpm1 = omega_e1 * 30 / pi / PMSM_Param.p;

syms id iq
eqns = [PMSM_Param.lambda*id + (id^2-iq^2)*(PMSM_Param.Ld-PMSM_Param.Lq) == 0, ...
    Tmax - 1.5*PMSM_Param.p*iq*(PMSM_Param.lambda+(PMSM_Param.Ld-PMSM_Param.Lq)*id) == 0];
S = solve(eqns, [id, iq], 'Real', true);
index = (double(S.id)<0) & (double(S.iq)>0);
id = double(S.id(index)); iq = double(S.iq(index));
omega_e2 = sqrt(Udc^2 / 3 / ((PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2));
rpm2 = omega_e2 * 30 / pi / PMSM_Param.p;

syms id iq
eqns = [(PMSM_Param.Ld-PMSM_Param.Lq)*((PMSM_Param.Lq*iq)^2 - (PMSM_Param.Ld*id+PMSM_Param.lambda)^2) ...
    - PMSM_Param.Lq*PMSM_Param.lambda*(PMSM_Param.Ld*id+PMSM_Param.lambda) == 0, ...
    id^2 + iq^2 == imax^2];
S = solve(eqns, [id, iq], 'Real', true);
index = (double(S.id)<0) & (double(S.iq)>0);
id = double(S.id(index)); iq = double(S.iq(index));
omega_e3 = sqrt(Udc^2 / 3 / ((PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2));
rpm3 = omega_e3 * 30 / pi / PMSM_Param.p;

%% Control Strategy Selector
Te = 100;
rpm = 1000;

%% Maximum Torque Per Ampere (MTPA) Control
syms id iq
eqns = [PMSM_Param.lambda*id + (id^2-iq^2)*(PMSM_Param.Ld-PMSM_Param.Lq) == 0, ...
    Te - 1.5*PMSM_Param.p*iq*(PMSM_Param.lambda+(PMSM_Param.Ld-PMSM_Param.Lq)*id) == 0];
S = solve(eqns, [id, iq], 'Real', true);
index = double(S.id) < 0;
id = double(S.id(index)); iq = double(S.iq(index));
fprintf('Solved result: id = %.3f, iq = %.3f.\n', MTPA_id(iq), iq);

%% Flux-Weakning (FW) control


%% Maximum Current Control
nb = 4350;
we = PMSM_Param.p * pi / 30 * nb;
syms id iq
eqns = [id^2+iq^2==imax^2, ...
    (PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2 - (Udc/we)^2/3 == 0];
S = solve(eqns, [id, iq], 'Real', true);

% eqns = [(PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2 - (Udc/we)^2/3 == 0, ...
%     Te - 1.5*PMSM_Param.p*iq*(PMSM_Param.lambda+(PMSM_Param.Ld-PMSM_Param.Lq)*id) == 0];
% S = solve(eqns, [id, iq], 'Real', true);
